#include "swi.h"
#include "vdu.h"
#include "fastmap.h"
#include "i2c.h"

#define PRINTSWI { printf("\nSWI Address: 0x%08x (%08x)\n", (state->Reg[15]-8) & 0x3fffffc, swicode); }

static bool vdu_enabled = true;

void WriteI(ARMword data)
{
	vdu_putchar( data & 0xff);
}

void Write0(ARMul_State *state)
{
	ARMword addr = state->Reg[0];

	FastMapEntry *entry = FastMap_GetEntryNoWrap(state, addr);
	FastMapRes res = FastMap_DecodeRead(entry,state->FastMapMode);
	 
	if(FASTMAP_RESULT_DIRECT(res))
	{
		char *ptr = (char *)FastMap_Log2Phy(entry,addr);
		int size = vdu_printf("%s", ptr);
		state->Reg[0] += (size +1);
	}
}

void WriteS(ARMul_State *state)
{
	ARMword addr = (state->Reg[15]-4) & 0x3fffffc;

	FastMapEntry *entry = FastMap_GetEntryNoWrap(state, addr);
	FastMapRes res = FastMap_DecodeRead(entry,state->FastMapMode);
	 
	if(FASTMAP_RESULT_DIRECT(res))
	{
		char *ptr = (char *)FastMap_Log2Phy(entry,addr);
		int count = vdu_printf("%s", ptr);
		
		ARMword newaddr = addr + count;
		
		if (newaddr & 0x3)
		{
			newaddr &= 0x3fffffc;
			newaddr += 4;
		}
		
		ARMul_SetPC(state, newaddr);
		state->Reg[14] = newaddr + 4;
	}		
}

void WriteN(ARMul_State *state)
{
	ARMword addr = state->Reg[0];
	FastMapEntry *entry = FastMap_GetEntryNoWrap(state, addr);
	FastMapRes res = FastMap_DecodeRead(entry,state->FastMapMode);
	 
	if(FASTMAP_RESULT_DIRECT(res))
	{
		char *ptr = (char *)FastMap_Log2Phy(entry, addr);
		unsigned int count = state->Reg[1];
		
		while (count)
		{
			vdu_putchar(*ptr++);
			count--;
		}
	}
}

extern struct I2CStruct I2C;


void OSByte161	(ARMul_State *state)
{
   ARMword result = I2C.Data[state->Reg[1]];
	
	if (state->Reg[1] == 185)
	{
		result = 0;
	}
	
	state->Reg[2] = result;
}

void OSByte129(ARMul_State *state)
{
	if (state->Reg[2] == 0xFF)
	{
		if (state->Reg[1] == 0x00)
		{
			state->Reg[1] = 0xA4;
			state->Reg[2] = 0x00;
		}
		else 
		{
			// TODO: support key scanning???
			state->Reg[1] = 0x00;
			state->Reg[2] = 0x00;
		}
	}
	else if (state->Reg[2] < 128)
	{
		int data = vdu_getchar();
		
		if (data > 0)
		{
			state->Reg[1] = data;
			state->Reg[2] = 0;
		}
		else
		{
			state->Reg[2] = 255;
		}
	}

}

void OSByte3(ARMword streams)
{
	vdu_enabled = !(streams & 0x01);
}

bool OSByte(ARMul_State *state)
{
	bool result = false;
	
	switch (state->Reg[0])
	{
		case 3:
		{
			OSByte3(state->Reg[1]);
			break;
		}
		case 129:
		{
			result = true;
			OSByte129(state);
			break;
		}
		case 161:
		{
			result = true;
			OSByte161(state);
			break;
		}
		
		default:
		{
			//fprintf(stderr, "OS Byte: %i, %x\n", state->Reg[0], state->Reg[1]);
			break;
		}
	}	
	
	return result;
}

/*@brief
 * 
 * returns true if the SWI is consumed, otherwise false.
 */
bool SWIHandler(ARMul_State *state, ARMword swicode)
{
	bool result = false;
	
	int error = swicode & 0x20000;
	
	if ((swicode & 0xFC0000) == 0)
	{
		switch (swicode & 0x1ffff)
		{
			case (OS_ReadC):
			{
				int data = vdu_getchar();
				if (data < 0)
				{
					data = 0;
				}
				
				state->Reg[0] = data;
				result = true;
				break;
			}
			case (OS_WriteC):
			{		
				char c = (char)(state->Reg[0] & 0xFF);
				vdu_putchar(c);
				break;
			}
			case (OS_WriteS):
			{	
				result = true;
				WriteS(state);			
				break;
			}
			case (OS_Write0):
			{	
				result = true;
				Write0(state);			
				break;
			}
			case (OS_WriteN):
			{	
				result = true;
				WriteN(state);			
				break;
			}
			case (OS_WriteI):
			{
				result = true;
				WriteI(swicode);			
				break;
			}
			case (OS_Byte):
				result |= OSByte(state);
				break;
			case (OS_Word):
				//fprintf(stderr, "OS Word: %i, %x\n", state->Reg[0], state->Reg[1]);		
				break;
			default:
				//fprintf(stderr, "Unhandled SWI: %06x\n", swicode);
				break;
		}
	}
	
	return result;
}
